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steering  and  stabilizing  robots  that  are  subject  to  nonholonomic  constraints. 
In  particular,  behaviors  for  robots  are  formalized  in  terms  of  kinetic  state  ma¬ 
chines,  a  motion  description  language  and  the  interaction  of  the  kinetic  state 
machine  with  information  coming  in  from  (limited  range)  sensors.  This  al¬ 
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Chapter  1 


Introduction 


Traditional  robot  motion  planning  and  obstacle  avoidance  concentrated  on 
determining  a  path  in  the  presence  of  holonomic  or  integrable,  equality  and 
inequality  constraints  on  the  configuration  space.  In  the  works  of  [1]  dy¬ 
namic  path  planning  algorithms  are  suggested  which  use  sensory  information 
to  generate  paths  assuming  no  a  priori  information  on  the  size  and  shape  of 
the  obstacles.  In  [2]  a  graph  search  algorithm  is  presented  to  find  obstacle 
free  paths  for  a  known  environment. 

On  the  other  hand  in  the  work  of  [3,  4,  5]  we  see  a  different  approach 
to  the  holonomic  motion  planning  problem  using  potential  functions.  The 
idea  behind  the  approach  in  [5]  was  to  associate  signed  charges  with  the  goal, 
robot  and  obstacles  and  then  generate  a  gradient  field  whose  integral  curves 
would  steer  the  robot  towards  the  goal  which  is  constructed  to  be  the  global 
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minimum  of  the  aggregate  potential.  Among  the  problems  encountered  in 
these  approaches  were  those  of  undesirable  local  minima  and  cycles,  when 
one  considers  intersecting  obstacles. 

To  solve  the  problem  of  obstacle  avoidance,  in  recent  years  we  have  seen 
significant  research  in  AI  in  the  areas  of  reactive  control,  one  form  of  which 
is  behavior  based  robotics.  Unlike  earlier  systems  which  relied  a  great  deal 
on  knowledge  representation,  reactive  systems  rely  on  the  direct  coupling  of 
sensory  information  and  actuators.  We  see  some  examples  of  reactive  planning 
systems  in  [6]  where  Brooks  uses  “task  achieving  behaviors”  as  the  primary 
decomposition  of  the  problem  of  autonomous  navigation.  He  introduces  the 
concept  of  subsumption  architecture  which  is  essentially  a  structured  and 
layered  set  of  behaviors  with  increasing  levels  of  competence.  Each  layer  is 
composed  of  augmented  finite  state  machines.  Other  approaches  along  similar 
lines  are  seen  in  [7,  8]  where  a  motor  schema  based  approach  is  suggested  for 
obstacle  avoidance  and  path  planning. 

In  practice  however  most  robotic  systems  include  constraints  that  are  not 
holonomic.  The  kinematic  constraints  cannot  be  reduced  to  equivalent  con¬ 
straints  on  the  configuration  variables  i.e.  constraints  cannot  be  integrated 
to  give  constraints  which  are  explicit  functions  of  position  variables.  A  few 
examples  of  such  systems  are,  models  of  a  front  wheel  drive  car,  dextrous  ma¬ 
nipulation  or  assembly  with  robotic  hands,  attitude  control  of  a  satellite  e.t.c. 
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Steering  and  stabilization  of  systems  subject  to  nonholonomic  constraints  are 
being  studied  extensively.  It  is  well  known  that  these  drift  free  completely 
nonholonomic  systems,  where  the  number  of  controls  is  less  than  the  number 
of  states,  are  controllable.  Papers  [9,  10,  11,  12]  presented  analytical  tools 
based  on  Lie  algebras  to  generate  control  sequences  to  steer  these  systems. 
As  these  nonholonomic,  drift  free  systems  do  not  satisfy  Brockett’s  necessary 
condition  for  smooth  stabilization  [13],  these  systems  cannot  be  stabilized  by 
using  smooth  time-invariant  state  feedback.  This  motivated  the  design  of 
piecewise  smooth  feedback  controllers  [14],  time- varying  periodic  controllers 
[15]  and  explicit  control  design  to  generate  time- varying  stabilizable  control 
laws  [16].  While  most  of  the  above  research  on  steering  and  stabilization  of 
nonholonomic  systems  assumes  an  obstacle-free  world,  we  note  that  the  prob¬ 
lem  of  autonomous  path  planning  and  obstacle  avoidance  with  nonholonomic 
robots  is  a  nontrivial  one.  Traditional  planners  assume  that  arbitrary  motion 
is  permitted,  and  hence  they  cannot  be  applied  to  nonholonomic  robots  as  they 
result  in  nonfeasible  paths,  i.e.  trajectories  that  do  not  satisfy  the  constraints 
on  the  configuration  variables.  This  motivates  the  need  for  a  hybrid  control 
strategy  that  integrates  features  of  traditional  planners  and  reactive  systems 
with  techniques  that  have  been  successful  for  control  based  approaches.  In 
[17,  18,  19]  possible  solutions  to  solve  this  problem  are  presented. 

In  this  thesis  we  present  a  path  planner  to  solve  the  problem  of  real-time 
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obstacle  avoidance  and  path  planning  with  nonholonomic  robots.  Unlike  ear¬ 
lier  approaches  the  planner  integrates  features  of  reactive  planning  systems 
with  modern  control  theory  approaches  to  steer  and  stabilize  nonholonomic 
robots.  Planning  is  restricted  to  the  two  dimensional  domain.  The  plan¬ 
ner  assumes  that  the  robot  has  limited  range  sensors  and  information  of  the 
coordinates  of  the  goal  and  its  own  coordinates  at  any  instant  of  time.  No 
restrictions  are  placed  on  the  size  and  shape  of  the  obstacles.  As  the  first 
step  towards  the  design  of  the  planner  we  introduce  a  formal  language  for 
motion  planning  in  which  we  model  the  robot  as  a  kinetic  state  machine.  The 
language  enables  us  to  define  and  reinterpret  some  of  the  existing  notions  of 
“behaviors”,  “plans”  etc.  We  then  introduce  a  hybrid  control  strategy  that  is 
motivated  by  the  hierarchical  and  distributed  nature  of  neuromuscular  con¬ 
trol.  Planning  is  done  at  two  levels  -  global  and  local.  For  local  planning 
obstacle  free  (non)feasible  paths  are  generated  using  potential  functions  as¬ 
suming  that  the  robot  is  a  point  robot.  A  feasible  path  is  then  generated  that 
obeys  the  constraints  in  the  configuration  variables.  As  feasible  trajectories 
are  only  approximations  to  the  trajectories  generated  using  potential  func¬ 
tions  collision  with  obstacles  while  tracing  the  feasible  trajectory  is  avoided 
by  encoded  sensor  information  in  the  kinetic  sate  machine.  In  addition  this 
information  is  also  present  in  a  lower  level  feedback  loop  while  the  robot  is  in 
motion.  At  a  global  level  heuristics,  along  with  the  world  map  generated  while 
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the  robot  is  en  route  to  the  goal,  are  used  to  solve  the  problem  of  cycles  en¬ 
countered  by  using  potential  functions  and  also  to  improve  the  “performance” 
of  the  planner  in  situations  where  the  same  or  similar  tasks  may  have  to  be 
repeated. 

The  thesis  is  organized  as  follows.  In  chapter  2  we  discuss  some  issues  re¬ 
lated  to  the  controllability  and  steering  of  nonholonomic  systems  with  specific 
reference  to  a  cart  with  a  two  wheel  drive  and  a  car.  We  also  review  some  of 
the  recent  work  done  in  feedback  stabilization  of  nonholonomic  systems.  In 
chapter  3  we  introduce  the  notion  of  a  kinetic  state  machine  and  attempt  to 
formalize  the  notion  of  behaviors  and  plans  for  obstacle  avoidance.  In  chap¬ 
ter  4  we  present  a  new  control  architecture  that  integrates  the  features  of 
behavior-based  control  systems  and  modern  control  theory.  We  also  present 
an  algorithm  for  obstacle  avoidance  and  present  some  results  from  the  sim¬ 
ulations  performed  to  test  the  algorithms.  Details  on  the  simulator  are  also 
provided.  Chapter  5  is  the  conclusion  and  discusses  some  of  the  future  work. 
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Chapter  2 


Controllability  and  Feedback 
Stabilization  of  Nonholonomic 
Systems 


A  mobile  robot,  whose  instantaneous  configuration  is  completely  determined 
by  a  set  of  generalized  co-ordinates  in  the  configuration  space,  is  often  sub¬ 
ject  to  multiple  constraints  including  geometric  constraints  and  kinematic 
constraints.  Kinematic  constraints  are  expressed  as  relations  between  gener¬ 
alized  coordinates  and  their  time  derivatives.  If  these  constraints  are  explicitly 
integrable  giving  the  form  ai(x )  =  ki  for  some  constant  hi,  where  x  denotes 
generalized  coordinates,  then  the  motion  of  the  system  is  restricted  to  a  level 
surface  of  a*  and  the  constraints  are  called  “holonomic”.  In  many  mechani¬ 
cal  systems  it  may  not  be  possible  to  write  the  kinematic  constraints  as  an 
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algebraic  equality  on  the  configuration  space,  i.e.  the  constraints  may  not  be 
integrable.  Such  constraints  are  called  “nonholonomic”  and  in  this  chapter  we 
will  review  some  properties  related  to  the  controllability  and  feedback  stabi¬ 
lization  [9,  10]  of  these  nonholonomic  systems.  We  assume  that  the  reader  is 
familiar  with  concepts  such  as  manifolds  and  vectorfields  (see  [20]  for  a  good 
exposition  of  these  concepts). 

2.1  Nonholonomic  Constraints 

Let  us  consider  a  mechanical  system  whose  configuration  space  is  an  n-  di¬ 
mensional  connected  simply  connected  manifold  M.  Let  us  represent  each 
configuration  of  the  system  by  an  n-dimensional  vector  x  —  (xi,X2,-"xn) 
where  Xi,  X2,  •  •  ■  xn  are  the  generalized  coordinates  of  the  configuration  space. 
Let  the  open  set  C  SR"  be  the  set  of  all  possible  configurations  of  the 
system  represented  in  the  local-coordinates  and  let  the  motion  of  the  sys¬ 
tem  be  represented  by  a  smooth  time  function  x{t).  The  tangent  vector  to 
the  trajectory  at  the  point  x  is  represented  by  x  =  (aq(f),  ^(t),  ■  •  •  ,xn(t)) 
where  X\ (t),X2(t),- •  •  ,xn(t)  are  the  generalized  velocities.  We  now  consider 
two  kinds  of  constraints  that  the  system  could  be  subject  to. 

1.  Classical  Geometric  Constraints  -  These  are  analytical  relations  between 
the  generalized  coordinates.  If  the  mechanical  system  is  subject  to  m  such 
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constraints,  then  there  exists  an  m-dimensional  vector  function  \k(x)  :  Q,  — >• 
such  that  'L(x)  =  0,  Vx  €  f2.  If  the  Jacobian  matrix  of  'I’(x)  has  full  rank, 
then  the  constraints  are  independent  and  m  generalized  coordinates  may  be 
eliminated  and  n  —  m  coordinates  are  sufficient  to  describe  the  system. 

2.  Kinematic  Constraints  -  These  constraints  are  represented  by  relations 
between  generalized  co-ordinates  and  their  velocities,  written  in  matrix  from 
as 


At{x)x  =  0  (2.1) 

where 

A{x )  =  (o1(®),a2(x),---o*(x)) 

and  aiT,  •  •  • akT  are  smooth  n-dimensional  covector  fields  on  M.  In  the  rest 
of  the  discussion  we  assume  that  associated  geometric  constraints  have  been 
eliminated  and  (2.1)  represents  the  system  with  m  independent  constraints. 

Let  A  be  a  smooth  nonsingular  distribution  that  annihilates  the  codistri¬ 
bution  given  by  the  covector  fields  oiT,  •  •  •  OfcT  i.e. 

A  =  span{bi,  b2,  ■  •  • ,  bm},  m  =  n  —  k  (2.2) 

where  &i,  b2,  •  •  • ,  bm  are  a  set  of  m  =  n  —  k  smooth  vector  fields,  that  satisfy 
the  relation 

a0T (x)bi(x)  =  0,  Vx  €  Q,  j  =  1,  ■  ■  •  k,  i  =  1,  •  •  •  m. 
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Let  B(x)  be  the  full  rank  matrix  made  up  of  the  vector  function  bi(x)  i.e. 
B(x )  =  (b{(x),  •  •  -6m(a;)).  Now  (2.1)  may  be  expressed  as 

x  G  A(ar)  or  x  G  Im(B(x))  (2.3) 

Along  the  trajectories  of  the  system,  equation  (2.3)  implies  the  existence  of  a 
vector  time  function  u(t)  G  for  all  t  such  that 

x  —  B(x)u(t)  (2.4) 

where  B(x)  is  the  full  rank  matrix  as  defined  above.  Equation  (2.4)  implies 
that  for  any  initial  condition  2c(0)  and  any  time  function  u(t)  the  solution  of 
x(t)  of  (2.4)  will  satisfy  (2.1). 

Hence  the  system  as  defined  by  (2.4)  can  be  viewed  as  a  control  system 
with  an  n-dimensional  state  space  representation  of  a  nonholonomic  system 
with  state  x(t)  and  control  u(t).  Systems  of  the  form  (2.4)  are  referred  to  as 
drift  free  systems  and  have  some  nice  properties  due  to  their  symmetry,  e.g. 
any  trajectory  can  be  followed  in  either  direction  by  changing  the  sign  of  the 
control. 


2.2  Controllability  of  Nonholonomic  Systems 

We  now  address  the  issue  of  controllability  of  systems  defined  by  (2.4)  i.e. 
x  =  bi(x)u\  +  •  •  •  bm(t)um  x  G  3?n,  u  G  3?m. 
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We  are  interested  in  determining  conditions  for  the  existence  of  controls 
u  :  [0,  T]  — >■  SRm,  T  >  0  that  steer  the  system  from  any  given  initial  state 
x0  =  :z(0)  to  Xf  =  x(T)  where  x0,Xf  G  fi.  Controllability  of  (2.4)  can  be 
characterized  in  terms  of  the  Lie  algebra  generated  by  the  vector  fields 
The  Lie  bracket  of  two  vector  fields  bi  and  bj  is  defined  as 

[bi'bj]=dibi~dZbj  (2-5) 

* 

We  now  define  A  as  the  involutive  closure  of  the  distribution  of  A  (as 
defined  in  (2.2  ))  under  Lie  bracketing.  Then  the  conditions  for  controllability 
are  given  by  Chow’s  theorem. 

* 

Theorem  1  (Chow)  [10]  If  A  (x)  =  for  each  x  G  S2  then  the  system 
(2.4)  is  controllable. 

A  useful  interpretation  of  Chow’s  theorem  can  be  obtained  by  using  the 
following  characterization  of  the  Lie  bracket.  Let  <fi{  :  C  >->•  !iftn  denote 
the  flow  of  a  vector  field  for  a  time  t.  Consider  the  sequence  of  flows  shown 
in  the  Fig  2.1.  The  net  motion  satisfies 

K9  0  4>7f  0  <t>9i  0  ([{(.Xo)  =  e2[f,  g]  +  o(e2) 

In  other  words  if  we  can  move  in  every  direction  using  Lie  bracket  motion 
the  system  is  controllable. 
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9 

Ef 

Figure  2.1:  Lie  bracket  motion 

2.2.1  Equations  of  Motion  for  the  Robotic  Cart 

Let  (x,  y)  £  5ft2  denote  the  position  of  the  robot  w.r.t.  some  inertial  frame  and 
9  £  Sl  (see  Fig.  2.2)  denote  the  orientation  of  the  robot  relative  to  the  x-axis. 
Hence  the  configuration  space  of  the  robot  is  Q  =  5ft2  x  S1.  Let  u\,u2  £  5ft 
denote  the  angular  velocities  of  the  wheels  .  Hence  the  kinematic  equations 
of  the  cart  are 

vx  =  x  =  ^  (lj i  +  <jj2)  cos  6  =  u\  cos  9 

vy  =  y  =  |(a>i  +  w2)  sin#  =  uisin#  (2-6) 

9  =  ftui-u  2)  =  U2 

where 

r  r 

Ul  _  2  2 

t  r 

U2  l  U2 
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Y 


Figure  2.2:  Robotic  Cart 


Rewriting  (2.6)  as 


(  \ 

(  ,  A 

(  \ 

X 

cos(0) 

0 

Ycart  • 

V 

= 

sin(0) 

Ui  + 

0 

.  0  , 

This  system  Ycart  is  a  drift  free  system  and  is  of  the  form  (2.4).  We  see  that 
{bi,  &2,  [h,  h]}  spans  9i3  and  hence  the  system  is  controllable. 


2.2.2  Equations  of  Motion  for  a  Front- Wheel  Drive 
Car 

Let  ( x ,  y)  G  5R2  denote  the  position  of  the  front  wheel  drive  car  w.r.t.  to  some 
inertial  frame,  9  G  S1  denote  the  orientation  of  the  car  relative  to  the  x  axis 
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Y 


Figure  2.3:  Car-like  Robot 

and  let  <j>  G  S1  denote  the  steering  angle  (see  Fig  2.3).  Hence  the  configuration 
space  of  the  car  is  Q  =  K2  x  S1  x  Sl .  The  constraints  for  the  front  and  rear 
wheels  are 

x  sin(0  —  4>)  —  y  cos(0  —  </>)  =  0 

—  (x  —  l  cos  9)  sin  0  —  —  l  sin  9)  cos  9  —  0 

at  at 

Let  v  e  U  the  heading  velocity,  and  (j)  E  3?  the  steering  velocity,  be  the  inputs 
to  the  system.  The  kinematic  equations  of  the  car  written  in  the  state  variable 
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form  are 


/  \ 

(  ,  \ 

(  \ 

x 

cos  (9  -  cf>) 

0 

y 

sin  (9  —  (j)) 

0 

= 

v  + 

9 

^  sin  0 

0 

U  J 

.  0  j 

(2.7) 


This  system  Y,Car  is  a  drift  free  system  and  is  of  the  form  (2.4).  We  see  that 
{hi,  b2,  [hi,  62],  [hi,  [hi,  h2]]}  spans  3ft4  and  hence  the  system  is  controllable  . 


2.3  Steering  of  Controllable  Systems 

One  of  the  solutions  suggested  to  steer  these  systems  is  driving  them  by  peri¬ 
odic  functions  [10].  For  example  consider  the  system  £cart  that  was  discussed 
in  the  previous  section.  Note  that  this  system  is  exactly  the  same  as  the 
unicycle  [12].  Now  once  again  modifying  the  input  to 

ni  =  ui  cos  9 
v2  -  u2 

and  relabeling  the  states  ( x\  =  x,  x2  =  9,  x3  =  y )  we  have 

xi  -  Vi 

x2  =  v2  (2-8) 

x3  =  v2  tan  x2 

Since  the  states  xi  and  x2  are  independently  controlled  by  inputs  v\  and  v2  we 
can  steer  xx  and  x2  to  the  desired  values.  This  may  cause  x3  to  drift.  To  steer 
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x3  to  its  desired  value  we  choose  V\  =  asm  (out)  and 
for  x\  we  have 


x3  =  tan(—  sin0)o;sin#d# 

UJ 


Observe  that  the  value  of  x3  after  —  advances  by 


j3  cos  (tut).  Solving 


(2.9) 


Hence  a 
value. 


1  r*  fj 

—  /  tan(— sin^Wsin 
27T  J — 7r  klu  ’ 


P. 


ddd 


/3,  and  co  can  be  selected  appropriately  to  steer  x3  to  the  desired 


2.4  State  Feedback  Control 

In  this  section  we  discuss  the  existence  of  smooth  pure  state  feedback  control 
to  stabilize  of  the  form  (2.4).  The  discussion  is  essentially  a  review  of  some 
aspects  of  papers  [9,  13]. 

A  pure  state  feedback  control  law  is  defined  as  a  smooth  mapping 

u  :  — >•  W1  :  u  -4  u(x ) 

with  the  additional  condition  that  u(0)  =  0.  The  application  of  this  feedback 
law  to  (2.4)  results  in  closed  loop  dynamics  of  the  form 

x  =  B(x)u(x) 

which  has  the  origin  as  the  equilibrium  point. 
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From  the  previous  discussion  of  controllability  of  (2.4)  we  know  that  there 
exists  a  control  law  that  will  drive  the  system  to  the  origin,  but  this  does  not 
ensure  the  existence  of  a  smooth  pure  state  feedback  law  that  will  asymp¬ 
totically  stabilize  the  origin.  In  fact  there  exists  no  pure  state  feedback  law 
that  asymptotically  stabilizes  the  origin.  Before  we  state  a  proof  to  show  that 
there  exists  no  pure  state  feedback  to  stabilize  the  origin  we  state  (without 
proof)  the  necessary  condition  for  feedback  stabilization  given  by  Brockett 

[13]. 

Theorem  2  (Brockett)  Let  x  —  f(x,u )  be  given  with  f(x0, 0)  =  0  and 
/(.,.)  continuously  differentiable  in  a  neighborhood  of  (x0,0).  A  necessary 
condition  for  the  existence  of  a  continuously  differentiable  control  law  that 
makes  (£0,0)  asymptotically  stable  is  that: 

(i)  the  linearized  system  should  have  no  uncontrollable  modes  associated 
with  eigenvalues  whose  real  parts  are  positive. 

(ii)  there  exists  a  neighborhood  N  of  (x0, 0)  such  that  for  each  £  6  N  there 
exists  a  control  u^(.)  defined  on  [0,  oo)  such  that  this  control  steers  the  solution 
of  x  =  f(x,  u%)  from  x  =  £  at  t  =  0  to  x  =  x0  at  t  =  oo. 

(in)  the  mapping  7  :  Q  x  5Rm  — >  9?"  defined  by  7  :  (x,u)  — >•  f(x,u )  should 
be  onto  an  open  set  containing  0. 

Corollary  1  Given  a  nonholonomic  system  of  the  form  (2.4)  there  does  not 
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exist  a  smooth  pure  state  feedback  law  of  the  form  u\  —  u(x)  •  ■  ■  um  =  u{x), 
that  makes  the  equilibrium  point  x  =  0  asymptotically  stable. 


Proof:  From  the  smoothness  in  A(x),  as  defined  in  (2.1),  and  the  indepen¬ 
dence  of  the  constraints,  we  know  that  there  exists  a  neighborhood  Vo  of  the 
origin  in  $Rn,  such  that  a  given  a  set  of  m  rows  of  A{x)  are  independent  on 
Vo-  Without  loss  of  generality,  we  assume  that  the  first  m  rows  of  A(x)  are 
independent  on  Vo,  and  we  partition  A(x)  as  follows 


A(x) 


Ai(x) 

v  Mx) 


\ 


) 


(2.10) 


where  Ai(x)  is  a  square  matrix  on  Vo- 

Let  Vi  be  a  neighborhood  in  9fJm,  containing  the  origin,  and  let  V  be  the 
cartesian  product  of  V0  and  Vi.  Consider  the  mapping 


(x,u)  -4  g(x,u)  =  B(x)u 


(2.11) 


and  denote  as  W,  the  image  of  V  by  this  mapping  g.  The  for  any  a  belonging 
to  W,  3  x  such  that  a  E  Im(B(x ))  and  therefore 

A?  \{x)  a  i  +  At2(x)<72  =  0  (2.12) 

where  a  is  partitioned  in  a  m-subvector  cti  and  a  (n  —  m)-subvector  <r2.  This 
implies  that  any  a  ,  with  o\  not  equal  to  zero  and  <r2  equal  to  zero,  does  not 
belong  to  W,  and  therefore  that  W,  the  image  of  the  open  set  V,  is  not  in  the 
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open  neighborhood  of  the  origin.  The  result  then  follows  from  Theorem  2 (in) 
(Brockett’s  necessary  condition  for  the  existence  of  smooth  feedback)  □ 

Theorem  3  [9]  With  the  smooth  feedback  control  law 

u(x)  =  —Bt(x)x 

the  equilibrium  point  x  =  0  of  the  closed  loop  system  is  globally  marginally 
stable.  Precisely: 

a)  the  state  x(t)  is  bounded  as  follows  for  all  t :  ||x(£)||  <  ||x(0)|| 

b)  the  state  x(t)  converges  to  the  invariant  set  U: 

U  =  {x  |  —  Bt(x)x  =  0} 

Proof.  Consider  the  Lyapunov  function  V (x)  =  xxT 

V{x)  =  -2 xTB(x)BT(x)x  <  0 

along  the  trajectories  of  the  closed  loop  system.  □ 

Example:  Consider  the  robotic  cart  given  by  (2.6).  The  feedback  law 

u\  =  —  x  cos  9  —  y  sin  6 
u2  =  -9 

stabilizes  the  system  to  the  invariant  set  U  —  {x  =  0,  y  —  k  6  9ft,  9  =  0}. 
Fig  2.4  shows  the  trajectories  of  the  system  with  the  above  feedback  law. 

In  path  planning  problems  in  many  situations,  e.g.  wall  tracing,  we  are 
interested  in  case  where  the  system  converges  to  trajectories  parallel  to  the 
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Figure  2.4:  State  Feedback  Stabilization 

the  boundaries  of  the  obstacles.  With  an  appropriate  change  of  coordinates 
in  the  case  of  a  car-like  robot,  this  problem  reduces  to  finding  a  control  law 
that  drives  the  system  to  the  set  U  =  {xi  E  5R,  x2  =  0,rr3  =  0,3:4  =  0}.  We 
suggest  a  control  law  and  an  algorithm  to  steer  a  car  like  robot  (2.13  -  2.16) 
(which  is  simply  (2.7)  with  new  variables)  to  the  invariant  set  U.  Before  we 
design  the  control  law  consider  consider  the  system  described  by  (2.15)  and 
(2.16) 


xi  =  cos(a;3  —  x^ui 

(2.13) 

±2  =  sin(x3  —  Xi)ui 

(2.14) 
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sinla^) 

£3  = - ; — Mi 


£4  =  u2 


(2.15) 

(2.16) 


Proposition  1  With  the  smooth  feedback  law: 

Ui  =  ki,  u2  =  k2  sin(j;3  —  £4),  k\,k2  €  5ft+  (2-17) 

the  origin  of  closed  loop  system  of  (2.15),  (2.16)  is  asymptotically  stable. 


Proof:  Let  D  =  {-n  <  x3  <  7r,  — 7t/2  <  -</w  <  X4  <  (f>max  <  +7r/2}. 
Consider  the  function  V  :  D  -4  3?  defined  by 


V(x)=arsinydy  +  bsin‘2l^Iit 
Jo  2 


Along  trajectories  of  the  closed  loop  system 


V(x)  —  asin^)^  +  26  sin  X-—  cos  ^"(£3  -  £4) 

Z  Z  Z 

J) 

—  ak2  sin(x4)  sin(a;3  —  X4)  +  -  sin(x3  —  X4)(—  —  sin(x4)  —  k2  sin(:r3  —  X4)) 

Z  L 

=  ( ak2  —  ^-k  1)  sin^)  sin(a;3  -  X4)  —  ^k2  sin2(:E3  —  X4) 

Zl  z 


Choose  a  =  | b  =  k2  then 


V{x)  =  -^2-  sin2(x3  —  X4) 
<  0 


20 


Let  S  be  the  set  of  points  where  V(x )  =  0.  Hence  S  =  {2:3,0:412:3— 2:4  =  0}. 
Let  us  assume  that  x(t)  is  a  trajectory  that  belongs  to  S  for  all  t. 

k\ 

=>  X3  —  24  =  — —  sin^)  —  k2  sin^  —  24)  =  0 
6 

=>•  2:4  =  0,  =>•  2:3  =  0 

Thus  the  only  solution  that  can  stay  in  S  is  the  trivial  solution  and  hence  it 
follows  from  La  Salle’s  Invariance  Principle  that  the  origin  is  asymptotically 
stable.  □ 

Since  (2:3, 2:4)  =  (0,0)  is  an  asymptotically  stable  equilibrium  point,  by 
continuity  there  exists  a  ball  Bs  =  {2:3,  2:4  6  2:4)  <  5}  such  that 

1 1 Z3  -  2:4 1 1  is  small  and  the  system  can  be  approximated  by 


2:3  =  -k\X\ 

xA  =  £2(2:3  -  2:4) 
Written  as  a  second  order  system  is  of  the  form 

ko 

y  +  k2y  +  -^-y  =  0 


(2.18) 


(2.19) 


which  is  the  equation  of  a  damped  oscillator.  Hence  we  can  choose  k\  and  k2 
and  obtain  a  desired  rate  of  convergence.  Note  that  once  in  Bs,  k2  determines 
the  rate  of  convergence,  but  £1  determines  the  rate  at  which  the  states  enter 
Bs.  Fig.  2.5  show  the  plots  of  the  inputs  and  states  for  the  same  initial 
conditions  and  different  values  of  £1  and  £2. 
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Now  observe  that  with  the  given  feedback  law  (2.14)  and  (2.16)  have  the 
same  RHS  and  hence 

ki  ky 

x2(t)  =  £2(0)  +  — £4(t)  -  t-^£4(0)  (2.20) 

k>2 

Since  the  closed  loop  system  (2.15  -  2.16)  is  asymptotically  stable, 

lim  xAt)  =  0 

hence, 

k, 

lim  x2(t)  =  £2(0)  -  — £4(0)  (2.21) 

t— >00  /c  2 

Observing  that  the  state  £4  can  be  steered  directly  by  control  u2  and  we 
have  the  freedom  to  choose  the  gains  k\  and  k2,  we  can  steer  the  car  to  the 
invariant  set  U  —  {£1  G  K,  x2  —  0,  £3  =  0,  £4  =  0}. 

Steering  algorithm 

(i) .  Depending  on  the  initial  condition  £2(0)  and  £4(0),  and  desired  conver¬ 
gence  rates  choose  gains  k\  and  k2. 

(ii) .  Apply  the  control  law  iti  =  0,  u2  —  c,  c  G  9?  for  time  t  =  (£2(0)|^  — 
£4(0 ))/c. 

(iii) .  Apply  the  control  law  (2.17). 

Observe  that  since  we  have  the  freedom  to  choose  the  gains  in  practical  appli¬ 
cations  the  closed  loop  control  can  be  turned  off  in  some  finite  time.  Fig.  2.6 
and  Fig  2.7  show  the  application  of  the  control  algorithm. 
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Chapter  3 


Behavior  Based  Reactive  Planning 
Systems 


In  chapter  2  we  discussed  some  control  laws  for  steering  and  stabilizing  drift 
free  nonholonomic  systems.  The  control  laws  discussed  assumed  that  no  ob¬ 
stacles  are  present.  Modeling  obstacles  as  constraints  in  the  configuration 
space  and  then  designing  control  laws  can  be  a  fairly  complex  problem.  Ear¬ 
lier  approaches  used  simple  open  loop  control  laws  or  “behaviors”  such  as 
move  or  turn  in  a  reactive  way  to  steer  these  robots.  Unlike  earlier  systems 
which  relied  a  great  deal  on  knowledge  representation,  reactive  systems,  rely 
on  the  direct  coupling  of  sensory  information  and  actuators.  We  see  some 
examples  of  reactive  planning  systems  in  [6]  where  Brooks  uses  “task  achiev¬ 
ing  behaviors”  as  the  primary  decomposition  of  the  problem  of  autonomous 
navigation.  He  introduces  the  concept  of  a  subsumption  architecture  which  is 


26 


essentially  a  structured  and  layered  set  of  behaviors  with  increasing  levels  of 
competence.  Each  layer  is  characterized  by  augmented  finite  state  machines 
[6].  But  having  had  a  better  understanding  of  the  properties  of  nonholonomic 
systems,  as  in  chapter  2,  one  would  like  to  exploit  the  underlying  geometry 
along  with  real  time  sensor  information  for  path  planning  and  obstacle  avoid¬ 
ance.  Hence  there  is  a  need  for  a  language  that  can  capture  and  integrate 
features  of  modern  control  theory  and  reactive  planning  systems.  Motivated 
by  [21,  22]  in  this  chapter  we  describe  a  formal  language  for  path  planning 
and  obstacle  avoidance.  The  language  also  gives  us  the  means  to  formalize, 
concepts  such  as  “behavior”,  “plan”  etc.  used  in  the  path  planning  literature. 

3.1  Kinetic  State  Machines 

We  treat  a  robot  as  a  kinetic  state  machine  [21]  which  can  be  thought  of  as  a 
continuous  analog  of  a  finite  automaton.  Kinetic  state  machines  are  governed 
by  differential  equations  of  the  form 

m 

x  =  ^2bi(x) Ui\  y  =  h(x)eW  (3.1) 

i= 1 

where 

z(-)  :  [0,  T]  -»■  Un 
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Ui  :  ?R+  xW  -»■  ft 


(t,y(t))  Ui(t,y(t)) 

Further  b{  is  a  vector  field  in  ft". 

We  now  define  the  atoms  of  the  motion  language  as  triples  of  the  form 
(U,  £,  T)  where 


U  —  (r^i)  ■  ■  ■  rtm) 

where  iq  is  as  defined  earlier, 

{0,1} 

s{t)  £(«(*)) 

is  a  boolean  function,  T  e  ft+  and  s(-)  :  [0 ,T]  — >  is  a  k  dimensional 

signal  that  represents  the  state  of  the  k  sensors  and  £  can  be  interpreted  as 
an  interrupt  to  the  system  which  is  activated  in  a  case  of  emergency,  e.g.  the 
robot  crashes  into  an  obstacle,  or  gets  too  close  to  an  obstacle.  Let  us  denote 
t,  0  <  t  <  T  as  the  time  at  which  the  interrupt  was  received  i.e.  £  changes 
state  from  1  to  0. 

If  at  time  to  the  kinetic  state  machine  receives  an  input  atom  (U,  £,  T)  the 
state  will  evolve  governed  by  the  differential  equation  (3.1),  as 


x  —  B(x)U,  V  t,  t0  <  t  <  t0  4-  min[t,  T } 


.  If  the  kinetic  state  machine  receives  an  input  string  (U\,  £i,  T\)  •  •  •  (Un,  £„,  Tn) 
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then  the  state  x  will  evolve  according  to 


x  —  B(x)Ui,  t0<t<  t0  +  min[ti,Ti\. 

_  (3-2) 

x  =  B(x)Un,  t0  +  min[ti,Ti ]  H - h  Tn_i] 

<t<  t0  H - hmm[tn,Tn]. 

Hence  we  may  denote  a  kinetic  state  machine  as  a  seven-tuple  ( U ,  A,  5, 5,  h,  £), 

where 

U  =  (CO0(9fi+  x  5ip);^ftm)  is  an  input  (control)  space, 

X  —  5?n  is  the  state  space, 
y  —  is  an  output  space, 

S  C  is  the  sensor  signal  space, 

B  is  an  K"xm  matrix  (constraints  matrix), 
h  :  X  -»  y  maps  the  state  space  to  the  output  space  and 
£  :  S  — >  {0, 1}  maps  the  sensor  signal  to  the  set  {0, 1}. 

Definition:  Given  an  atom,  ([/,  £,  T),  define  (aU,£,  /3T),  a  G  €  S ft+  as 
the  corresponding  scaled  atom  and  denote  it  as  (a,  /3)(U,£,T). 

Definition:  An  alphabet  E  is  a  finite  set  of  atoms,  i.e  (U,£,T)  triples. 
Thus  E  =  {(Ui,£i,ti),  ■  •  • ,  (Un,£n,tn)}  for  some  finite  n  or  equivalently  E  = 
{(Ti,  •  •  •  an}  where  c*  denotes  the  triple  (17*,  £*,  T*),  such  that  cr*  ^  (a,  fd)(oj)  a  G 
$R,  /3  G  3?+  and  i  =  1,  •  •  -n,  j  =  1  •  •  *n. 
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Definition:  An  extended  alphabet  £e  is  the  set  of  scaled  atoms,  i.e  (all,  f ,  @T) 
triples  derived  from  the  alphabet  £. 

Definition:  A  language  £*  (£*)  is  defined  as  the  set  of  all  strings  over  the 
fixed  alphabet  £  (extended  alphabet  £e). 

Definition:  A  behavior  it  is  an  element  of  the  extended  language  £*.  For 
example  given  an  alphabet  £  =  {<7i,  er2}  a  behavior  7r  could  be  the  string 
(oi,  (3i)a1(a2,  @3)01- 

Remark:  To  account  for  constraints  one  might  limit  behaviors  to  lie  in  a 
sublanguage  B  C  £*.  This  will  be  explored  in  future  work. 

To  simplify  notation  we  denote  the  scaled  atom  (1,  l)oj  simply  by  a 
Definition:  The  length  of  a  behavior  denoted  by  \tt\  is  the  number  of  atoms 
(or  scaled  atoms)  in  the  behavior. 

Definition:  The  duration  T( 7r)  of  a  behavior 

•7 r  =  (ai,/?i)(C/i,^i,Ti)  •••(«„, Pn)(Un,€n,Tn) 

executed  beginning  at  time  t0  is  the  sum  of  the  time  intervals  for  which  each 
of  the  atoms  in  the  behavior  was  executed.  That  is, 

T( 7r)  =  t0  +  min[ti,PiTi] - h  min[tn,  (3nTn ]  (3.3) 

Definition  :  Given  a  kinetic  state  machine  and  a  world-model,  a  plan  T  is 
defined  as  an  ordered  sequence  of  behaviors,  which  when  executed  is  guaran¬ 
teed  to  achieve  the  given  goal.  For  example  a  plan  T  =  {7r37ri7rn  •  ■  •}  could  be 
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generated  from  a  given  language  where  each  behavior  is  executed  in  the  order 
in  which  they  appear  in  the  plan.  The  length  of  a  plan,  |T|  =  ^”=1  W-  The 
duration  of  the  plan  is  given  by  T(T)  =  X)”=i  Tfc).  In  a  particular  context 
there  may  be  more  than  one  plan  that  achieves  a  given  goal. 

Remark:  Since  each  atom  when  executed  by  a  kinetic  sate  machine,  combines 
in  general  both  open-loop  and  feedback  controls,  one  could  argue  that  our 
definition  of  behavior  captures  the  essence  of  behavior  (say  in  movement)  in 
biology,  as  well  as  the  sense  in  which  the  term  isused  by  Brooks  [6]. 

Given  a  nonholonomic  robot,  an  environment  and  a  certain  task,  some 
important  questions  that  arise  are  - 

(i)  How  does  one  choose  an  alphabet  E,  or  even  a  more  elementary  question  - 
does  there  exist  a  E  which  can  be  used  to  generate  behaviors  and  hence  plans 
to  achieve  the  required  goal  ? 

(ii)  Given  an  alphabet  set  of  behaviors  is  infinite.  For  practical  reasons  one 
might  want  to  work  with  a  finite  subset  of  behaviors.  How  does  one  choose 
such  a  finite  subset  of  behaviors  ? 

(iii)  As  there  may  be  more  than  one  plan  that  steers  the  robot  to  the  goal 
one  might  be  interested  in  formulating  some  notions  of  performance  (optimal¬ 
ity)  to  plans  ? 
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Proposition  2  Given  an  obstacle-free  environment  and  a  kinetic  state  ma¬ 
chine  that  is  governed  by  the  differential  equation 

m 

x  —  bi(x)ui]  x  e  9?”,  u  6  5Rm  (3.4) 

2—1 

such  that  the  control  Lie  algebra  (i.e.  the  vector  space  spanned  at  any  point 
by  all  the  Lie  brackets  of  the  vector  fields  bi)  has  rank  n,  then  there  exists  an 
alphabet  £(£*)  which  can  be  used  to  generate  behaviors  and  hence  plans  to 
steer  the  system  from  a  given  initial  state  x0  to  a  final  state  Xf. 

Proof:  (i)  From  Chow’s  theorem  we  know  that  if  the  control  Lie  Algebra  has 
rank  n  then  the  system  is  controllable.  This  implies  there  there  exist  controls 
u  :  [0,T]  — >  $lm,T  >  0  that  steer  the  system  from  any  initial  state  xo(0)  to 
any  final  state  Xf(T). 

A  simple  alphabet  that  can  be  used  to  generate  behaviors  consists  of  m  triples 
of  the  form  {U\,  1, 1),  •  •  • ,  ( Um ,  1, 1)  a  €  3?,  (3  G  where 

Ui  =  (1,0, 0,0,  •••,0) 

u2  =  {  0, 1,0,0, •••,<>) 

□ 

um  =  (0,0,0,  •  •  •  ,0, 1) 

Example  1.  Consider  the  problem  of  path  planning  with  a  unicycle,  with  a 
single  sensor,  that  wanders  around  in  a  given  environment  without  colliding 
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into  obstacles  (analogous  to  the  idea  of  the  zeroth  level  of  competence  in 
Brooks  [6]  ).  Let  us  assume  the  task  of  the  robot  (unicycle)  in  this  case  is  to 
wander  till  it  senses  an  obstacle.  If  it  senses  an  obstacle  it  avoids  the  obstacle 
and  continues  to  wander  around.  We  now  formulate  and  solve  this  problem 
treating  the  unicycle  with  its  sensor  as  a  kinetic  state  machine  and  find  a  plan 
that  solves  the  problem.  The  differential  equations  governing  the  kinetic  state 
machine  are 


v\  cos  6 

(3.5) 

V\  sin  6 

(3.6) 

V2 

(3.7) 

where  (x,y)  G  9?2  denotes  the  position  of  the  unicycle  w.r.t  some  inertial 
frame,  9  e  S1  denotes  the  orientation  of  the  unicycle  relative  to  the  hori¬ 
zontal  axis,  V\  and  t>2,  the  velocity  of  the  unicycle  and  the  angular  velocity 
respectively  are  the  inputs  to  the  kinetic  state  machine  .  To  solve  this  problem 
let  us  consider  the  following  atoms: 
o i  =  (UutuTi)  where 


t/i  =  (l,0) 


6  = 


< 


1 


0 


if  p  >  10 
if  p  <  10 


Ti  €  (0,  oo) 
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where  p  is  the  distance  between  the  robot  and  the  obstacle  that  is  returned 


by  the  sensor. 
a 2  =  {U2,€2,T2)  where 

E4  =  (0,l) 

{0  if  p  >  10 
1  if  p  <  10 
T2  G  (0,  oo) 

=  {U3,£3,T3)  where 

C/3  =  (0, 1) 

6  =  1 

T3  G  (0,  oo) 

Let  ot  G  ftpiai]  and  G  [0,  ooj. 
now  consider  the  following  behaviors 

7T!  =  (ai\#)(t/i,6,l) 

7T2  =  (a\,a\)(U2,  6, 1) 

7t3  =  (of?,^)(t/3,<e3,  i) 

Based  on  the  equations  of  this  robot,  the  behavior  ni  is  interpreted  as  “move 
forward”  with  a  velocity  of  a\  units/sec  for  (3\  seconds  and  behaviors  7t2  and 
7t3  can  be  interpreted  as  “turn”  with  a  velocity  of  a\  deg/sec  for  maximum 
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of  seconds  i.e.,  turn  right  by  a  maximum  of  (5  degrees,  unless  interrupted. 
As  explained  earlier  the  atoms  of  each  behavior  will  only  execute  as  long  as 
their  respective  £  functions  are  1  and  the  time  of  execution  is  less  than  T. 


time 

Figure  3.1:  Trajectory  and  Inputs  Generated  by  the  Plan 

Consider  the  following  plan  T  =  ((5, 100)7Ti(— 1,  90)7t3)*  i.e. 

T  =  {(g^/^tti  tt3  (a^&Vi  •  •  •} 

If  this  plan  is  executed  in  the  environment  as  shown  in  the  Fig.  3.1  observe 
that  the  robot  will  move  forward  for  time  t,  t  <  100  when  £i  will  interrupt  it. 
The  execution  of  behavior  ni  is  inhibited,  behavior  7 r3  is  picked  up  from  the 
queue  and  is  executed.  As  £3  =  1  in  the  entire  interval  t  €  [t,  90]  the  robot 
will  then  turn  clockwise  by  90  degrees  and  then  it  will  move  forward  (execute 
behavior  7Ti).  But  again  after  some  finite  time  wall  W2  (see  Fig.  3.1)  will 
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cause  £1=0  and  hence  interrupt  the  move  forward  behviour.  Behavior  7 r3 
is  executed  as  earlier  i.e.  the  robot  turns  clockwise  by  90  degrees,  and  now 
continues  to  move  forward.  If  it  does  not  detect  an  obstacle  at  the  end  of 
100  seconds  since  it  started  moving  forward,  it  will  stop  turn  clockwise  by  90 
degrees  and  continue  to  repeat  its  behavior. 

Now  consider  the  plan  V  =  ((50,  2)7Ti(— 20,  5)7t2)*.  If  this  plan  is  executed 
in  the  same  environment  (see  Fig.  3.2)  observe  that  while  executing  the  “move 
forward’  i.e.  7Ti,  in  the  time  interval  0  <  t  <  2  the  robot  realizes  that  the 
obstacle  is  at  a  distance  less  than  10  units  from  it  and  hence  £1  interrupts 
the  “move  forward  ”  and  the  robot  begins  to  execute  “turn  right”.  Due  to 
the  choice  of  the  interrupt  function  £2  robot  will  now  switch  between  “turn 
right”  and  and  “move  forward”  (a  condition  reffered  to  as  chattering)  and 
trace  a  trajectory  as  shown  in  the  figure.  Hence  depending  on  the  choice  of 
the  alphabet  one  can  generate  different  plans  to  achieve  the  same  task. 

The  question  of  how  to  generate  a  plan  given  an  alphabet  and  a  kinetic 
state  machine,  is  an  open  one  and  it  largely  depends  on  the  task.  In  the 
next  chapter  we  describe  a  path  planner  for  nonholonomic  robots.  Before  we 
discuss  the  features  of  the  planner  we  introduce  some  more  definitions  that 
help  formalize  measures  to  evaluate  the  performance  of  a  plan. 
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Figure  3.2:  Trajectory  and  Inputs  Generated  by  the  Plan 

3.2  Performance  Measure  of  a  Plan 

To  generate  a  plan  to  steer  a  system  from  a  give  initial  state  x0  to  a  final 
state  Xf  requires  complete  a  priori  information  of  the  world,  which  is  not 
always  the  case  in  most  instances  of  path  planning.  If  we  assume  that  the 
system  does  not  have  complete  a  priori  information  about  the  world  W  then 
the  planning  system  has  to  generate  a  sequence  of  plans  based  on  the  limited 
information  about  W  that  it  has  which  when  concatenated  will  achieve  the 
required  goal.  We  call  these  plans  that  are  generated  on  limited  information 
to  achieve  some  subgoal  as  Partial  Plans  P\  The  plan  to  steer  the  system 
from  a  given  initial  state  x0  to  a  final  state  Xf  is  then  determined  after  the 
system  has  reached  the  final  state  and  is:  T  =  r!Pr2p  •  •  •  Vnv  where  T/  is  the 
partial  plan  consisting  of  only  those  behaviors  that  have  been  executed  for 
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t  >  0. 


Remark:  As  the  partial  plan  is  generated  with  limited  information  of  the 
world,  all  the  behaviors  generated  by  the  partial  plan  may  not  be  executed 
in  real  time.  For  example  let  us  consider  a  partial  plan  =  {7r37ri7r4  •  •  -i r„}. 
Let  us  assume  that  the  behavior  ixi  is  interrupted  by  £1  at  t.  Now  as  by  the 
definition  of  the  plan  behavior  7r4  will  begin  to  execute.  But  if  £1  =  £4  the 
behavior  7T4  will  not  be  executed. 

The  length  of  a  plan  is  given  by  |T|  =  £”=1  |7Ti|  and  the  time  of  execution  of 
the  plan  is  given  by  T(r)  =  £”=1T(7r j). 

Now  that  we  have  these  formal  definitions  we  can  start  defining  the  per¬ 
formance  of  an  algorithm  that  uses  these  behaviors  and  analyze  some  earlier 
algorithms  for  nonholonomic  motion  planning. 

Given  an  algorithm  that  generates  a  plan  T  we  define  a  measure  of  per¬ 
formance  0(r)  of  the  plan  as 

0(r)  =  r(r)  +  r|r|  (3.8) 

where  r  is  a  normalizing  factor  having  the  units  of  time. 

Observe  that  the  performance  of  a  plan  implicitly  depends  on  the  kinetic 
state  machine,  which  determines  the  choice  of  the  behavior  ir. 

Defining  a  performance  measure  for  a  path  planner  is  a  rather  difficult  task 
as  it  is  largely  dependent  on  the  goal  the  robot  has  to  achieve.  Some  path 
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planners  use  the  total  time  to  achieve  the  goal  as  a  measure  of  the  performance 
of  the  system.  In  many  situations  one  might  be  interested  in  not  only  the 
time  but  also  on  the  smoothness  of  the  path  traversed  or  the  number  of  times 
switching  between  different  controls  was  necessary.  For  example  consider  the 
task  of  parallel  parking  of  a  car.  One  might  be  able  to  achieve  the  goal  by 
using  only  open  loop  controls  but  switching  between  them  at  regular  intervals, 
hence  possibly  reducing  the  time  to  achieve  the  goal  but  compromising  on  the 
smoothness  of  the  path.  On  the  other  hand  if  one  uses  a  time  dependent 
feedback  law,  the  same  task  could  be  possibly  achieved  by  moving  along  a 
smooth  trajectory  but  this  time  taking  a  longer  time  to  achieve  the  goal. 
This  indicates  a  trade  off  between  the  two  strategies  which  is  captured  by  the 
performance  measure  defined  by  (3.8). 

We  now  define  the  optimal  performance  of  a  plan  as 

@(r)0ptima*  =  mm{T(r)  +  r|r|}.  (3.9) 

Here  the  minimization  is  performed  over  the  subset  of  plans  defined  by  the 
subset  of  behaviors.  Depending  on  the  kinetic  state  machine  and  the  choice 
of  the  planner  one  can  now  place  bounds  on  the  optimal  performance  and 
hence  compare  the  performance  of  different  planners  given  the  same  language 
or  that  of  the  planner  given  a  new  language.  This  is  illustrated  in  the  example 
given  below. 
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Example  2:  Consider  the  problem  of  steering  the  unicycle  from  a  given  initial 
location  z0  to  Zf.  The  equations  of  the  unicycle  are  given  in  example  1.  Let  us 
assume  that  the  language  consist  of  the  following  atoms.  o\  —  (Ui,  £1, 1),  o2  — 
(172,6 2)1)  where  are  as  defined  in  example  1.  Let  a  E  [—5, +5] 

and  (3  E  (0,  oo) 

Let  us  also  assume  that  the  planner  did  not  have  complete  information 
about  the  world  and  had  to  generate  n  partial  plans  to  achieve  the  goal. 
Each  partial  plan  consists  of  steering  the  unicycle  from  Z;  to  Zj  (see  Fig  3.3) 
such  that  there  are  no  obstacles  in  some  small  neighborhood  of  the  the  line 
segment  joining  these  two  locations.  Let  us  further  make  an  assumption  that 
the  planner  uses  on  E  [1,-1]  as  the  scaling  factor  while  generating  partial 
plans. 

From  the  kinematic  equations  of  the  unicycle  we  know  that  a  simple  partial 
plan  to  steer  a  unicycle  from  Zj  =  ( X{ ,  yi,  9i )  to  Zj  =  ( Xj,yj ,  9j )  would  be  : 

(i)  turn  by  ( 6ZiZj  -  9i), 

(ii)  move  by  a  distance  d*  and 

(hi)  finally  turn  by  ( 9f  -  9ZiZj), 

where  ZiZj  is  the  vector  joining  z*  and  Zj,  di  =  ||zjZjj|2  and  9ZiZj  is  the  orien¬ 
tation  of  the  vector  w.r.t.  to  the  x-axis. 

We  can  rewrite  this  simple  algorithm  as  a  partial  plan  derived  from  the 
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Figure  3.3:  Partial  Plan  Generation 
language  using  the  behaviors  7Ti  =  ai  and  7r2  =  cr2  . 

r pi  =  {(On/abs(dn),  dn )(a2),  (1,  di)(oi),  ( di2/abs(ei2 ),  ^2)^2)} 

where  0jiand  ()l2  are  the  angles  of  the  two  turns  as  described  above  of  the  ith 
partial  plan.  Hence  the  plan  to  steer  the  system  from  z0  to  Zf  is  given  by 

r  =  {r^rp2...rpn} 

Given  a  plan  we  now  illustrate  how  bounds  can  be  placed  on  the  optimal 
performance  based  on  the  knowledge  of  the  kinetic  state  machine  and  the 
language. 

Let  dmax  =  max  ||arjXj||.  But  since  the  planner  uses  a *  e  [—1,1] 

T(rpi)max  <  27T  +  dmax  +  2tt 
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—  47r  -h  dmdx 

Since 

|rp*U  <  3 

0  <  Q(r)  <  3n  +  n(4n  +  dmax ) 

But  since  we  are  using  only  open  loop  controls  we  know  from  the  kinematics  of 
the  system  that  given  an  inital  state  x0  and  a  final  state  x /  both  the  behaviors 
(ai,j3i)ai  and  (/co:*,  fii(k)<Ji  would  steer  the  kinetic  state  machine  from  the 
intial  state  to  the  final  state,  we  could  replace  by  (kai,  f3i/k)cri. 

Hence  from  our  assumption  that  the  planner  uses  only  oti  £  [—1,1]  and 
observing  that  if  in  our  language  a*  €  [—5, 5]  it  implies  that 

o<0(rw<^+3» 

Having  placed  bounds  on  a  plan  generated  by  one  set  of  behaviors  we  can 
now  compare  the  performance  of  another  set  of  behaviors  (may  be  one  using 
periodic  functions  to  steer  the  robot)  against  these  bounds. 
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Chapter  4 


Path  Planning  and  Obstacle 
Avoidance 


We  have  seen  in  the  earlier  chapters  that  path  planning  for  nonholonomic 
systems  with  (time  dependent)  state  feedback  involves  both  open  loop  and 
closed-loop  controls.  This  motivates  the  need  for  an  intelligent  trade  off  be¬ 
tween  closed  loop  and  open  loop  control  for  tasks  of  path  planning  and  obstacle 
avoidance.  In  chapter  3  we  developed  a  formal  language  for  planning  which 
integrates  features  of  modern  control  theory  and  behavior  based  planning  as 
discussed  in  chapter  3.  In  this  chapter  we  present  an  architecture  for  real  time 
implementation  of  the  language  and  a  general  purpose  algorithm  for  obstacle 
avoidance  with  nonholonomic  robots.  The  specific  details  of  the  algorithm  are 
described  using  a  robot  that  is  designed  along  the  lines  of  a  unicycle  and/or 
a  car  with  a  front  wheel  used  for  steering  and  driving  purposes.  The  equa- 
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tions  of  motion  and  closed  loop  feedback  laws  that  were  described  in  chapter 
2  are  used  where  explanation  using  an  example  is  required  for  a  better  under¬ 
standing.  The  algorithm  makes  certain  assumptions  on  the  world,  and  certain 
pragmatic  assumptions  about  the  sensing  capabilities  of  the  robot  which  are 
consistent  with  many  real-time  applications  using  mobile  robots.  In  the  rest 
of  this  chapter,  plans,  partial  plans,  behaviors  and  atoms  are  used  in  the  sense 
as  defined  in  chapter  3. 


4.1  Control  Strategy 

As  we  seek  to  incorporate  higher  levels  of  autonomy  in  robots,  the  need  for 
hierarchical  and  distributed  control  schemes  that  have  a  biological  analog 
becomes  apparent.  Based  on  our  current  knowledge  of  the  organization  of  the 
mamalian  motor  systems  we  might  consider  segments  of  the  limbs  as  kinetic 
state  machines,  with  motor  commands  to  the  muscles  and  tendons  and  sensor 
information  (that  enters  into  a  low  level  feedback  at  the  spinal  level)  as  inputs 
to  the  machine.  Motivated  by  the  hierarchical  structure  of  neuromuscular 
control  [23]  and  observing  that  the  low  level  spinal  reflex  control  runs  faster 
(loop  delays  of  about  30ms)  than  the  high  level  feedback  loop  (100-200  ms 
delays),  we  present  a  control  scheme  (see  Fig.  4.1),  to  generate  and  execute 
plans  to  achieve  a  given  task. 
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SENSORS 


Kinetic  State  Machine 


Figure  4.1:  Hybrid  Control  Architecture 
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The  lowest  level  (the  kinetic  state  machine  with  sensors  and  (U,  f ,  T)  in¬ 
puts)  could  be  interpreted  as  a  low  level  feedback  controller  operating  at  the 
spinal  level.  The  planner  could  be  interpreted  as  the  higher  end  of  the  nervous 
system  (cortex  +  memory..)  where  sensory  information  has  been  processed 
to  generate  goal  related  trajectory  information.  The  distributed  nature  of  the 
control  becomes  apparent  when  one  observes  that  once  a  plan  has  been  gener¬ 
ated  each  level  and  even  various  modules  at  the  same  level  (e.g.  cleanup  and 
plan  in  Fig  4.1)  continue  to  execute  independently.  As  explained  in  chapter 
3,  if  the  kinetic  state  machine  receives  an  input  ( U{ ,  £j,  Tj)  it  evolves  according 
to  equation  3.2.  Interpret  Tj  as  a  timer  whose  output  is  1  (active  high)  while 
t  <  Tj  and  is  0  (active  low)  if  t  >  Tj.  As  explained  earlier  £(s(t))  is  a  boolean 
function  that  returns  an  interrupt  (active  low)  to  the  system  when  conditions 
defined  by  £{s(t))  are  satisfied.  Hence  the  functioning  of  the  AND  gates  in 
the  kinetic  state  machine  can  be  interpreted  as  follows  -  if  either  the  robot 
receives  an  interrupt  or  t  >  Tj,  the  input  to  gate  II  is  an  active  low  and  hence 
the  input  to  the  kinetic  state  machine  is  inhibited  i.e.  the  current  behavior  is 
stopped  and  the  next  behviour  in  the  queue  is  executed. 

It  is  assumed  that  the  robot  has  no  knowledge  about  the  shapes  of  the 
obstacles.  Planning  is  restricted  to  the  two  dimensional  cartesian  space.  The 
plan  generated  consists  of  an  ordered  sequence  of  partial  plans  each  of  which 
is  generated  based  on  the  limited  local  information  that  the  robot  has.  As 
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obstacles  are  encountered  en  route  to  the  goal  the  world  map  is  updated, 
and  a  new  path  to  the  goal  is  again  planned  using  the  new  information  now 
available  to  the  planner.  As  the  partial  plans  are  generated  based  on  local 
information  the  paths  initially  generated  are  locally  optimal  (in  the  sense  of 
chapter  3) .  As  the  knowledge  of  the  environment  increases  the  performance  of 
the  system  begins  to  improve.  Inaccuracies  in  the  plan  due  to  incorrect  sensor 
information  do  not  pose  any  problem  because  sensor  information  is  present  in 
the  low  level  feedback  loop  at  run  time  i.e.  while  the  plan  is  being  executed. 
The  environment  is  represented  as  a  network  of  free-space  regions. 

4.2  Navigation  Task  Composition 

The  task  of  navigation  and  obstacle  avoidance  can  be  decomposed  into  a 
number  of  subtasks  each  being  executed  at  a  certain  level  in  the  control  ar¬ 
chitecture.  Refer  to  Fig.  4.2  for  a  diagram  of  task  decomposition. 


Figure  4.2:  Navigation  Task  Decomposition 
Read  sensors  involves  reading  the  distance  information  returned  by  the 
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sensors  and  determining  the  the  largest  obstacle  free  disk  centered  around  the 
robot  such  that  the  trajectories  that  lie  in  this  disk  are  guaranteed  not  to 
intersect  with  the  boundaries  of  the  obstacle.  In  the  implementation  of  the 
control  strategy  the  sensors  are  read  continuously  and  distance  information 
and  p0fd  (the  radius  of  the  obstacle  free  disk)  are  updated  into  a  global  variable 
which  is  used  to  update  the  world  and  is  also  used  by  execute  plan  as  an 
input  to  the  interrupt  function  £. 

Since  the  environment  is  represented  as  a  network  of  free-space  regions, 
update  world  involves  periodic  addition  of  nodes  to  the  network.  In  the 
actual  implementation  of  the  planner  we  differentiate  between  the  local  world 
and  the  global  world.  The  local  world  is  represented  as  a  linked  list  of  inter¬ 
secting  obstacle-free  disks,  each  of  which  is  added  to  the  list  in  the  order  they 
were  visited  over  some  finite  interval  of  time.  Each  node  (see  Fig.  4.3)  con¬ 
tains  information  regarding  the  sensor  distance  information  and  p0fd.  Partial 

Struct  legal_highway 

{ 

float  sensorfl  0],  /  *  array  with  distance  information  of  each  IR  *  / 

rho_ofd,  /  *  radius  of  obstacle  free  disk  7 
center  [2],  /  *  x,  y,  coordinates  of  the  obstacke  free  disk  *  / 

struct  legal_highway  *  next_ofd, 

*  prev_ofd; 

} 


Figure  4.3:  Representation  of  the  World 


plans  use  this  information  regarding  the  obstacle-free  disks  to  generate  be- 
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haviors  that  steer  the  robot  to  the  boundary  of  the  disk.  After  the  complete 
or  partial  execution  of  the  partial  plan,  the  robot  generates  the  next  partial 
plan  based  on  the  sensor  information  available  at  that  instant.  Note  that  even 
though  the  sensors  are  continuously  being  monitored,  a  new  node  is  added  to 
the  list  only  when  a  new  partial  plan  needs  to  be  generated.  Hence  as  shown 
in  Fig.  4.4  the  world  is  updated  at  time  instances  corresponding  to  the  points 
A,  B,  C  and  so  on. 


Figure  4.4:  World  Update 

Generate  Plan/Partial  Plan  -  This  involves  the  interpretation  of  the 
sensor  information,  and  language  to  generate  a  sequence  of  behaviors  that  will 
steer  the  robot  from  its  current  location  to  a  desired  location.  If  a  partial  plan 
is  being  generated  the  desired  location  lies  on  the  boundary  of  the  obstacle 
free  disk.  The  generation  of  plans/partial  plans  is  discussed  in  further  detail 
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in  section  4.3. 


In  the  implementation  of  the  planner,  the  data  structure  of  a  plan  (see  Fig. 
4.5)  is  a  linked  list  of  atoms,  each  atom  being  represented  by  a  structure  that 
has  information  regarding  the  scaling  factors  (a,  (3),  the  interrupt  function 


the  maximum  time  of  execution  of  the  kinetic  state  machine,  the  inputs 
(controls)  and  a  pointer  to  the  kinetic  state  machine.  As  mentioned  earlier 


it  is  possible  that  while  executing  a  partial  plan  only  some  of  the  behaviors 
may  be  executed  as  planned,  some  may  be  executed  only  for  a  fraction  of  the 
the  intended  execution  time  and  others  may  not  be  executed  at  all.  Update 
Plan  updates  the  fields  of  each  atom  of  the  partial  plan  after  it  has  completed 


its  execution. 

Struct  plan 

{ 

float  max_exec_time,  /  *  maximum  time  of  execution  7 

act_exec_time,  /  *  actual  time  for  which  atom  was  executed  *  / 
alpha,  beta,  /  *  scaling  factors  associated  with  the  atom  *  / 

*  ( *  control )( );  /  *  Input  U  to  the  differential  equation  *  / 

int  *  ( interrupt)  ( );  /  *  interrupt  function  associated  with  the  atom  7 

void  *  (  KSM  )  ( );  /  *  equations  of  motions  of  the  robot  *  / 

struct  plan  *  next_plan, 

*  prev_plan; 

} 


Figure  4.5:  Representation  of  a  Plan 


Execute  Plan  involves  decomposing  these  plans  into  atoms  and  letting 


50 


the  kinetic  state  machine  evolve  accordingly.  It  involves  a  continuous  scan 
of  the  sensor  information  as  £(s(t))  requires  run  time  sensor  information  to 
inhibit  the  evolution  of  the  kinetic  state  machine. 

As  the  local  world  is  a  list  of  nodes,  each  of  which  is  added  to  the  list 
as  the  robot  generates  partial  plans  it  is  possible  that  while  generating  a 
plan  the  robot  may  have  to  revisit  a  node,  and  hence  introducing  redundant 
information  into  the  world.  Cleanup  keeps  track  of  these  redundancies  in  the 
map  and  deletes/adds  nodes  in  the  list.  It  also  uses  the  local  map  information 
available  and  builds  a  graph  model  of  the  world  map  which  can  be  later 
searched  using  standard  algorithms  like  A*  to  generate  optimal  paths  to  nodes 
on  the  graph.  This  module  is  currently  in  its  implementaion  stages. 

In  the  rest  of  the  discussion  we  restrict  ourselves  to  obstacle  avoidance  and 
path  planning  with  a  unicycle  and/or  a  front  wheel  drive  cart,  equipped  with 
a  finite  number  of  infrared  sensors  that  return  distance  information. 


4.3  Partial  Plan  Generation 

As  mentioned  earlier  a  partial  plan  should  consist  of  behaviors  that  steer  the 
robot  in  these  obstacle  free  disks  such  that  the  trajectories  do  not  leave  the 
disk,  hence  avoiding  obstacles,  and  the  direction  of  travel  should  be  such  that 
the  net  effect  of  the  partial  plan  is  to  steer  the  robot  towards  the  goal  in  a 
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global  sense.  We  would  also  like  to  generate  plans  that  have  nearly  optimal 
performance,  where  the  performance  measure  is  as  defined  in  chapter  2.  Hence 
assuming  that  for  the  given  language  there  exists  more  that  one  behavior  to 
steer  the  robot  to  the  given  subgoal,  the  planner  now  has  an  additional  task  of 
choosing  the  behavior  that  yields  a  close  to  optimal  performance.  This  could 
be  done  either  by  generating  all  possible  partial  plans  and  then  choosing  the 
one  with  an  optimal  performance  or  making  the  decision  based  on  a  set  of 
rules  which  depend  on  the  interpretation  of  the  environment.  For  example,  it 
is  intuitively  clear  that  to  steer  a  car  in  an  obstacle-free  disk  of  small  radius 
using  pure  open  loop  controls  would  result  in  a  significantly  large  number  of 
switching  between  controls  and  hence  resulting  in  a  path  of  high  complexity. 
In  this  situation  one  could  view  complexity  as  the  number  of  turns  the  robot 
has  to  make.  Before  we  go  into  the  actual  details  of  plan  generation  we 
introduce  some  definitions  and  assumptions  that  we  make  about  the  robot 
and  the  world. 

4.3.1  Assumptions  and  Definitions 

Assumptions: 

(i)  At  any  given  time  the  robot  has  information  about  its  current  coordinates 
and  the  coordinates  of  the  goal.  It  has  no  a  priori  information  on  the  location 
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-  INFRARED  /  ULTRASONIC  SENSOR  BEAM 

Figure  4.6:  Location  and  Calibration  of  IR  sensors 

and  shape  of  the  obstacles.  No  constraints  are  imposed  on  the  size  and  shape 
of  the  obstacles. 

(ii)  Location  and  calibration  of  sensors  on  the  robots:  For  purposes  of  obstacle 
detection,  ranging  and  mapping,  the  robots  are  provided  with  infrared/ultrasonic 
sensors  located  as  shown  in  the  Fig  4.6.  Bump  sensors  are  located  along  the 
body  of  the  robot.  Optical  encoders  mounted  on  the  motors  provide  position 
and  velocity  feedback.  The  infrared  sensors  are  classified  as  Sleye  if  they  are 
located  in  the  eye,  as  S\0dy  if  they  are  located  along  the  sides  or  as  Sltaii  if 
they  are  located  at  the  rear  of  the  robot  (see  Fig.  4.6).  The  sensors  return 
distance  information  and  we  shall  refer  to  the  distance  information  returned 
as  p\ye  or  p\ody  depending  on  which  sensor  detects  an  object  along  its  line 
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of  sight.  The  sensors  are  calibrated  such  that  the  maximum  range  of  each 
of  these  sensors  is  on  the  boundary  of  a  circle  of  radius  Rmax  centered  at  C. 
The  sensors  located  at  the  rear  of  the  robot  are  normally  turned  off  and  are 
turned  on  only  when  the  robot  is  reversing.  While  reversing  they  function  in 
a  manner  similar  to  those  of  the  eye. 

Definition  :  Obstacle  free  disk  BQfd{C,  p0fd)  is  defined  as  a  disk  of  radius 
Pofd  (Pofd  <  Rmax)  centered  at  C  (see  Fig.  4.6)  such  that  there  are  no 
obstacles  in  this  ball,  i.e.  p0fd  =  min [pleye,  P^bodyb  *  =  T  ••• n .  j  =  1  ...n.  Any 
trajectory  that  lies  within  this  ball  does  not  intersect  the  boundaries  of  any 
obstacle 

As  the  distance  between  the  robot  and  the  obstacles  decreases,  due  to  the 
nonholonomic  constraints  steering  might  become  more  difficult  and  the  plan¬ 
ner  might  need  to  adopt  a  different  control  scheme  to  steer  the  robot.  Let 
R-crt  <  Rmax  denote  the  radius  of  the  ball  centered  at  C  such  that  the  planner 
needs  to  use  a  different  set  of  controls  to  steer  the  robot  from  x0  to  Xf  €  dpafd- 
In  the  actual  implementation  of  the  planner  we  place  a  threshold  on  the 
radius  of  the  obstacle  free  disk  and  have  a  rule  according  to  which  if  p0fd  <  Rcrt 
then  we  use  closed  loop  controls  to  steer  the  robot.  Hence  the  problem  of 
generating  partial  plans  reduces  to  (i)  planning  in  an  obstacle  free  disk  with 
p0fd  >  RCrt  and  (ii)  planning  in  an  obstacle  free  disk  such  that  p0fd  <  Rcrt- 
Each  of  the  cases  is  discussed  in  the  following  subsections. 
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4.3.2  Planning  in  the  Obstacle  Free  Disk 

To  find  the  best  direction  of  travel  in  this  case  we  use  the  approach  of  potential 
functions.  As  in  the  earlier  work  on  path  planning  with  potential  functions  the 
idea  behind  our  construction  is  based  on  electrostatic  field  theory  -  charges  of 
the  same  sign  repel  and  charges  of  the  opposite  sign  attract  in  accordance  with 
Coulomb’s  law.  Hence  we  assign  a  positive  charge  distribution  to  the  obstacles 
and  the  mobile  robot  and  a  negative  charge  distribution  to  the  goal  and  assign 
an  artificial  potential  function  to  the  robot.  We  use  the  resultant  gradient  field 
only  to  determine  the  scaling  factors  and  Xf  E  dp0fd,  the  circumference  of  the 
obstacle  free  disk,  as  the  integral  curves  may  not  result  in  feasible  trajectories. 
The  idea  is  to  construct  a  vector  field  which  will  give  the  best  direction  of  travel 
based  on  the  location  of  the  obstacles  and  the  goal.  The  robot  is  approximated 
by  a  point  robot  and  sensors  can  detect  only  points  on  the  boundaries  of  the 
obstacles  that  lie  in  their  line  of  vision,  we  treat  obstacles  as  point  charges 
and  assign  charges  to  them  depending  on  which  sensor  detects  them.  The 
sum  total  of  both  the  attractive  and  repulsive  forces  is  used  to  determine  the 
bounds  on  the  velocities  and  hence  the  bounds  on  the  scaling  factors.  We  also 
assign  weights  to  the  forces  due  to  the  goal 

Let  \leye ,  body  be  charges  associated  with  points  on  the  boundaries  of 
the  obstacle/goal  detected  by  the  sensors  on  the  robot.  Depending  on  which 
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sensor  detects  them  we  assign  them  values  as  follows: 

(0  Ag  body  >  0  if  pP  body  <  Rmax 

(ii)  A  eye  >  body  >  0  if  (P eye  <  Rmax 

Observe  that  the  length  of  the  shortest  feasible  path  to  move  to  a  configuration 
that  lies  in  the  cone  of  the  eye  is  less  that  the  length  of  the  shortest  feasible 
path  to  a  configuration  that  lies  at  the  same  euclidean  distance  along  (near) 
the  body.  The  choice  of  \leye  >  A Pbody  ensures  that  if  an  obstacle  is  detected 
at  the  same  euclidean  distance  by  Sleye  and  S\ody  the  change  in  trajectory 
required  due  to  an  obstacle  detected  at  a  distance  p\ye  is  more  than  that 
required  if  an  obstacle  is  detected  at  p>bod  . 

We  associate  subgoals  (negative  charges)  when  no  obstacle  is  detected  by  a 
sensor  in  the  eye  to  suggest  a  possible  free  path  in  the  direction  of  motion. 

(iii)  A  eye  <C  0  if  p  eye  =  Rmax 

(iv)  The  gradient  field  associated  with  the  obstacles  is  given  by 

v/*  =  (£  AVV/iI!"  +  E  A 'm.V/,'”*) 

i=l  i=  1 

The  gradient  field  associated  with  the  goal  is 

V/9  =  \\Vfobs\\Ug 

ug  =  |||^ro^I^g°a|]||  where  Xrobot  and  Xgoai  E  5R2  are  the  coordinates  of  the 
(point)  robot  and  the  goal  respectively,  where 
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The  resultant  gradient  field  is  given  by: 


n  n 

F  =  (£  AVV/i'”'  +  £  A>w„V/(“=')  +  V/, 

i=l  i=l 

and  the  best  direction  of  travel  is  given  by  the  pp  The  intersection  of  this 
vector  with  the  boundary  of  the  obstacle  free  disk  determines  the  final  location 
to  which  the  robot  has  to  be  steered. 

Observe  that  in  the  obstacle  free  disk  the  resultant  gradient  field  has  only  one 
equilibrium  point,  the  center  of  the  obstacle  free  disk  and  it  occurs  when 

V  fobs  1 

In  such  a  situation  we  disturb  the  location  of  the  goal  by  a  small  amount. 
Having  calculated  the  location  on  the  boundary  of  the  disk  to  which  the  robot 

has  to  be  steered  we  use  control  laws  similar  to  those  discussed  in  chapter  2 
to  steer  the  robot. 

4.3.3  Tracing  Boundaries  of  Obstacles 

Planning  in  B(C,  Rcrt )  is  now  a  closed  loop  planning  strategy  which  essentially 
results  in  a  trace  behavior  that  traces  the  boundaries  of  the  obstacles.  An 
example  of  the  closed  loop  control  is  given  in  chapter  2.  Given  the  limited 
sensor  and  world  information  it  is  probable  that  the  direction  of  trace  may 
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have  been  wrong.  Hence  we  use  a  heuristic  function  f(x)  =  D(Xrobot,Xgoai), 
the  euclidean  distance  between  Xrobot  and  Xgoai  as  an  estimate  of  how  far  the 
robot  has  strayed  from  the  goal.  Trace  the  boundary  as  long  as  f  <  fs  where 
fs  is  the  euclidean  distance  D(Xrobot,Xgoai),  at  the  instant  when  the  trace 
behavior  was  started.  If  /  >  fs  then  retrace  path  and  trace  the  boundary  of 
the  obstacle  in  the  opposite  direction.  If  terminal  conditions  (trace  until  you 
find  a  corner)  for  trace  are  not  met  set  fs  =  2/s  and  repeat. 

Remark:  Retracing  a  path  under  this  framework  is  a  rather  simple  task. 
Observing  that  the  system  is  a  drift  free  system,  retracing  involves  executing 
the  past  n  partial  plans  in  a  reverse  order  with  (-a)  scaling  factor. 

4.4  Path  Execution 

Once  the  plans/partial  plans  have  been  generated,  these  plans  have  to  be  exe¬ 
cuted.  The  general  algorithm  for  path  execution  is  shown  in  Figure  4.7.  Plan 
execution  involves  decomposition  of  the  plan  into  behaviors  and  further  into 
atoms.  The  kinetic  state  machine  is  allowed  to  evolve  as  explained  in  Chapter 
3,  equation  3.2.  We  would  like  to  mention  here  that  we  use  two  levels  of  in¬ 
terrupts  in  the  implementation  of  the  path  execution  module.  While  a  partial 
plan  is  being  executed  if  a  low  level  interrupt  is  received  the  execution  of  that 
particular  atom  is  inhibited  and  the  actual  time  of  execution  of  the  atom  is 
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updated  for  future  use  and  the  next  atom  in  the  partial  plan  is  executed.  On 
the  other  hand  if  a  higer  level  interrupt  is  received  the  execution  of  the  partial 
plan  itself  is  inhibited  and  a  new  partial  plan  is  generated. 

Fig.  4.7  and  Fig. 4. 8  show  some  of  the  paths  generated  by  the  planner  using 
method  described  in  section  4.3.2.  It  should  be  pointed  out  here  that  the 
obstacle-free  disks  generated  by  the  planner  violate  the  definition  as  stated  in 
section  4.3.2,  but  this  is  because  in  the  simulator  we  have  used  only  sensors  of 
the  eye  to  generate  obstacle-free  disks.  These  obstacles  that  are  not  detected 
by  the  sensors  may  be  thought  of  as  being  the  blind  spots  of  the  robot.  It 
is  important  to  note  that  while  the  plan  is  being  executed  the  sensor  are 
being  continuously  scanned  and  are  present  in  a  low  level  feedback  loop  hence 
preventing  any  collisions  with  obstacles. 

4.5  Learning  and  World  Model  Update  Al¬ 
gorithm 

Once  the  robot  has  explored  the  environment  using  limited  range  sensors,  it 
is  natural  to  expect  the  robot  to  generate  plans  of  a  better  performance  if  it 
has  to  repeat  the  same  task  or  move  to  goal  that  lie  in  the  explore  regions. 
We  suggest  a  “learning  algorithm”  that  improves  the  performance  of  a  plan 
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Obstacles 


Obstacle  free  disks 


Figure  4.7:  Paths  Generated  by  the  Planner 
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Obstacles 


Obstacle  free  disks 


Figure  4.8:  Paths  Generated  by  the  Planner 
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to  bring  it  closer  to  optimal.  The  main  features  of  the  algorithm  are  discussed 
below. 

As  described  in  section  4.3,  the  plan  to  steer  a  robot  consists  of  a  sequence 
of  partial  plans,  where  each  partial  plan  steers  the  robot  in  some  obstacle  free 
disk  of  radius  p0fd .  In  the  rest  of  the  section  let  us  denote  each  of  the  obstacle 
free  disk  which  were  used  to  generate  the  ith  partial  plan  as  Bi  and  the  ith 
partial  plan  as  Tf.  Once  the  plan  has  been  generated  the  planner  makes  the 
following  observations.  Further  let  us  assume  that  n  such  partial  plans  were 
generated. 

(i) .  If  Bi  C  Bj ,  i  —  1  =  1  ,---n  (i.e.  Bi  is  contained  in  Bj )  then 

obviously  Bi  contains  redundant  information.  Thus  if  Bi  C  Bi+1  the  partial 
plan  Tf,  that  steers  the  robot  form  Ci  to  Cj+i,  where  Ci  is  the  center  of  the 
obstacle  free  disk,  and  partial  plan  rf+1  that  steers  the  robot  form  Cj+i  to 
Ci+ 2  can  be  replaced  by  a  partial  plan  Tf  that  steers  the  robot  from  Ci  to 
Ci+2 .  Since  Bi  C  Bi+1  it  is  obvious  that  @(ffi)  <  @(rfrf+1) 

(ii) .  Observe  that  since  Cj+i  lies  on  the  boundary  of  Ci,  we  are  guaranteed 
the  existence  of  a  trivial  nonfeasible  trajectory  (the  straight  line  joining  Ci 
with  Ci+2 )  that  lies  entirely  in  Bi  U  Bj+i  i.e.  the  obstacle  free  area  enclosed  by 
these  two  intersecting  obstacle  free  disks.  Hence  if  there  exists  a  partial  plan 
r?  that  generates  feasible  trajectory  that  can  track  this  nonfeasible  trajectory 
and  lie  entirely  in  Bi\J  Bi+\  such  that  ©(ff*)  <  ©(riTi+1)  we  can  replace 
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If  It,  by  ff 

(iii)  After  the  execution  of  (i)  and  (ii)  we  now  have  partial  plans  that  steer 
the  robot  from  Q  to  Q+j,  j  £  {2,  •  •  -  n}  such  that  the  trajectory  lies  entirely 
in  The  planner  now  explores  the  possibility  of  finding  (non)feasible 

trajectories  from  Cj  to  Ci+j+k,  j  £  2,  •  •  •  n,  k  =  1,  •  •  • ,  n  such  that  these  trajec¬ 
tories  lie  entirely  in  \Ji+J^kBl  and  the  performance  of  the  plan  that  generates 
this  trajectory  is  better  than  the  earlier  one.  In  Fig.  4.9  we  give  an  example 
of  an  algorithm  to  explore  the  possibility  of  a  trivial  (non)feasible  trajectory. 
Fig.  4.10  and  Fig.  4.11  show  paths  generated  by  the  planner  after  it  has  gained 
partial  knowledge  of  the  world  it  has  explored  in  its  first  attempt  to  reach  the 
goal.  It  clearly  shows  an  improvement  in  the  performance  of  the  planner 
as  the  length  of  the  plan  is  nearly  a  third  of  the  plan  generated  in  the  first 
attempt. 

Remarks:  (i).  One  should  note  that  generating  plans  of  better  performance 
does  not  necessarily  imply  that  |ff  |  <  |Tf  j  where  ff  is  the  new  plan  but  could 
simply  imply  choosing  the  right  scaling  factors  a,  (3  such  that  T(ff)  <  T(rf). 
(ii).  One  need  not  restrict  the  generation  on  nonfeasible  trajectories  to  straight 
line  segments,  but  could  instead  use  arc  or  even  curves  that  best  fit  the  centers 
of  these  obstacle  free  disks. 
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Iearn_update  ( ) 

{ 

k  =  1  ; 

while  (  k  !=  n  ) 

{ 

for  ( i  =  n  ;  i  >  k  ;  i  -- ) 

{ 

in_disk_flag  =  1  ; 

/  *  C  ( . )  is  the  coordinates  of  the  center  of  an  obstacle  free  disk  */ 

L  j  L  k=  eqnjine  (  C  ( i ) ,  C  (  k ) )  /  *  equation  of  the  line  joining  the 

C  ( i ) and  C  (  k ) * / 

for  ( j  =  i ;  j  >  k  ;  -j ) 

{ 

if  (  Lj  Lkn  B  (  C  ( j )  ==  0 )  /  *  i.e.  if  L  j  Lk  does  not  intersect  the 

obstacle  free  ball  centered  at  C(  j )  *  / 

{ 

in_disk_flag  =  0 ; 
break ; 

} 

} 

if(  in_disk_flag  ==  1) 

{ 

generate_partial_plan  (C(k),C(i)); 

/  *  if  there  exists  a  path  that  passes  through  all  the  obstacle  free 
disks  Bk  to  Bj  then  generate  a  partial  plan  to  steer  the  robot 
from  C  (  k )  to  C  ( i )  *  / 

break ; 

} 

} 

k  =  i  ; 

} 

} 


Figure  4.9:  Learning  and  Plan  Update  Algorithm 
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Figure  4.10:  Plan  Generated  after  Gaining  Partial  Knowledge  of  the  World 
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Figure  4.11:  Plan  Generated  after  Gaining  Partial  Knowledge  of  the  World 
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Chapter  5 


Conclusions  and  Future  Directions 


In  this  thesis  an  attempt  was  made  to  bring  together  aspects  of  motion  plan¬ 
ning  with  robots  as  seen  by  researchers  in  the  communities  of  reactive  planning 
and  control  theory.  Some  of  the  conclusions  are: 

•  An  investigation  into  the  strengths  and  drawbacks  of  the  approaches 
adopted  by  these  two  communities  to  solve  the  problem  of  autonomous 
motion  planning  suggests  that  a  hybrid  strategy  that  integrates  features 
of  both  might  provide  a  better  solution. 

•  Since  many  robotic  systems  include  constraints  that  are  not  holonomic, 
reducing  the  robot  to  a  point  and  then  designing  planning  and  naviga¬ 
tion  systems  for  such  robots  is  not  always  possible.  A  complete  under¬ 
standing  of  the  constraints  and  related  issues  (controllability,  feedback 
stabilization)  is  required  to  design  control  laws  to  steer  these  systems 
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along  feasible  trajectories.  Depending  on  the  world  in  which  the  robot  is 
navigating  in  many  situations  it  might  be  helpful  to  switch  from  steering 
using  analytical  tools  based  on  Lie  algebras  to  planning  that  relies  on 
the  direct  coupling  of  sensory  information  and  actuators. 

•  To  capture  features  of  continuous  and  discrete  time  control  strategies, 
an  interface  is  required  and  this  interface  is  provided  via  the  motion  de¬ 
scription  language  suggested  in  chapter  3.  The  language  serves  as  a  tool 
to  generate  control  laws  or  “behaviors”  that  integrate  real  time  sensor 
information  with  continuous  time  control  laws.  The  language  provides  a 
framework  to  encode  and  compare  various  control  strategies  (piecewise 
continuous  feedback  laws,  reactive  planning)  for  path  planning. 

•  As  we  seek  higher  levels  of  autonomy  in  robots  the  need  for  a  hierarchical 
and  distributed  control  schemes  that  have  a  biological  analog  becomes 
apparent.  The  architecture  presented  in  chapter  4  is  in  part  inspired  by 
some  understanding  of  the  mamalian  motor  system. 

•  Though  steering  and  stabilization  of  systems  subject  to  nonholonomic 
constraints  have  been  studied  extensively,  many  of  the  steering  algo¬ 
rithms  and  feedback  laws  suggested  cannot  be  applied  for  real  time  ap¬ 
plications.  To  implement  control  strategies  on  nonholonomic  robots  for 
real  time  applications  the  design  of  controllers  that  can  generate  control 
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sequences  that  result  in  “nice  trajectories”  is  essential. 


Future  research  directions  should  include: 

•  Continuing  the  formalization  of  behavior-based  robotics.  This  should 
include  expanding  the  concept  of  the  motion  description  language  to 
include  multiple  kinetic  state  machines  and  communication  protocols 
between  kinetic  state  machines. 

•  Design  of  analytical  tools  based  on  Lie  algebras  for  real  time  control  of 
nonholonomic  robots. 

•  Explore  the  possibilities  of  using  adaptive  neural  nets  to  generate  intel¬ 
ligent  behaviors  for  path  planning  with  moving  obstacles,  increasing  the 
levels  of  parallelism  in  the  control  strategy  for  more  efficient  real  time 
planning  and  include  error  recovery  strategies. 

•  continued  development  of  the  simulator  for  testing  the  integration  of 
control  and  reactive  techniques  in  complex  (moving  and/or  movable 
obstacles)  problems. 
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